import java.util.ArrayList;
import java.util.List;


public class Simulator{
	
	public static final Simulator instance = new Simulator().initialize();
	
	public static void main(String[] args) {
		run();
	}
	
	public Simulator initialize() {
		Data.saysFirst = 0; 
		Data.saysSecond = 0;

		Data.kingSeenMove = -1;
		
		Data.kingSeen = false;
		Data.princessSeen = false;
		
		Data.moveList = new ArrayList<Move>();
		Data.kingsPath = new ArrayList<Move>();
		
		Data.firstFloor = new Map(Constants.SIZE_FIRST_FLOOR_X, Constants.SIZE_FIRST_FLOOR_Y);
		Data.secondFloor = new Map(Constants.SIZE_SECOND_FLOOR_X, Constants.SIZE_SECOND_FLOOR_Y);
		
		Data.currentFloor = Data.firstFloor;
		
		return this;
	}
	

	public static void run() {
		Robot.initialize();
		Robot.start();
		Robot.setup(0, 0, Constants.RIGHT);
		ColorDetector.initialize();
			try {
				testFirstFloor();
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
			climbUpRamp();
			Data.roomNumber = 2;
			Robot.numMovesAfterCorrection = 0;
			try {
				Data.currentFloor = Data.secondFloor;
				Robot.setup(0, Data.currentFloor.yGrids - 1, Constants.UP);
				testSecondFloor();
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
				climbDownRamp();
			Robot.numMovesAfterCorrection = 0;
			try {
				Data.currentFloor = Data.firstFloor;
				Robot.setup(Data.currentFloor.xGrids - 1, Data.currentFloor.yGrids - 1, Constants.UP);
				returnToKing();
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
	}

	public static void testFirstFloor() throws InterruptedException {
		Robot.initialize();
		
		PathPlanner pathPlanner = new PathPlanner();
//		for(int i = 0; i < 40; i++)
//			pathPlanner.goToClosestUnseenGrid();
		pathPlanner.goTo(10, 3, false);
		
		if(Data.kingSeen){
			pathPlanner.generateKingsPath(false);
		}
		Data.moveList.clear();
		System.out.println(Data.saysFirst + " people say that princess in the first room.");
		System.out.println(Data.saysSecond + " people say that princess in the second room.");

		while(pathPlanner.probability() < 1){

			pathPlanner.goToClosestUnseenGrid();
			if(Data.kingSeen && Data.kingsPath.size() == 0){
				pathPlanner.generateKingsPath(true);
			}
		}
		
		if(Data.saysFirst>Data.saysSecond)
			Data.roomNumber = 1;
		else
			Data.roomNumber = 2;
		
		List<Move> turnBack = new ArrayList<Move>();
		pathPlanner.generatePath(turnBack, 0);
		pathPlanner.useGeneratedPath(turnBack);
		Data.moveList.clear();
	}
	
	public static void climbUpRamp() {
		Robot.rotateTo(Constants.RIGHT);
		Robot.robothw.correction(0);
		Robot.rotateTo(Constants.DOWN);
		Robot.robothw.goForward();
		Robot.robothw.correction(2);
		Robot.rotateTo(Constants.RIGHT);
		Robot.robothw.correction(0);
		Robot.rotateTo(Constants.LEFT);
		Robot.robothw.goForward();
		Robot.robothw.climbUp();
		Robot.rotateTo(Constants.DOWN);
		Robot.robothw.correction(0);
		Robot.rotateTo(Constants.UP);
		Robot.robothw.goForward();
	}
	
	public static void climbDownRamp() {
		Robot.robothw.goForward();
		Robot.rotateTo(Constants.DOWN);
		Robot.robothw.correction(2);
		Robot.rotateTo(Constants.LEFT);
		Robot.robothw.correction(0);
		Robot.robothw.climbDown();
		Robot.rotateTo(Constants.DOWN);
		Robot.robothw.correction(1);
		Robot.rotateTo(Constants.RIGHT);
		Robot.robothw.correction(0);
		Robot.rotateTo(Constants.UP);
		Robot.robothw.goForward();
	}
	
	public static void testSecondFloor() throws InterruptedException{
		PathPlanner pathPlanner = new PathPlanner();
		pathPlanner.findPrincess(Data.roomNumber);
	}
	
	public static void returnToKing() throws InterruptedException{
		PathPlanner pathPlanner = new PathPlanner();
		if(Data.kingSeen){
			pathPlanner.useGeneratedPath(Data.kingsPath);
		}
		else{
			while(!Data.kingSeen){
				pathPlanner.goToClosestUnseenGrid();
			}
			System.out.println("King seen");
		}
			
	}
}
